#include <stdio.h>
#include <unistd.h>
#include <errno.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <assert.h>
#include <fcntl.h>
#include <linux/videodev2.h> 

#include <ZLog.h>

#define CAPTURE_IMG_FMT V4L2_PIX_FMT_MJPEG
#define DEMO_IMAGE_PATH "./IMG_CAMERA_DEMO.jpg"
#define IMAGE_DUMP
#define IMAGE_WIDTH 1280
#define IMAGE_HEIGHT 720

#define CAMERA_DEVICE "/dev/video0"
#define MAP_BUF_NUM 4

struct CameraDeviceS
{
    int fd;
    int isUsbDev;
	struct v4l2_capability cap; 
	uint32_t format;

	int wight;
	int height;

    uint32_t codecFmt[16];
    int codecFmtCount;
    
	void *mapMem[MAP_BUF_NUM];
	int mapMemSize[MAP_BUF_NUM]; 
};

/*
char *FourCCToStr(int32_t value)
{
    static char buf[8] = 0;
    buf[0] = value & 0x000000ff;
    buf[0] = value >> 8 & 0x000000ff;
    buf[0] = value >> 16 & 0x000000ff;
    buf[0] = value >> 24 & 0x000000ff;
    return buf;                
}
*/
int GetCodecFmtList(struct CameraDeviceS *device)
{	
	struct v4l2_fmtdesc fmtdesc;
	int i = 0;

	device->codecFmtCount = 0;
	
	fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	for (i = 0; i < 16; i++)
	{
		fmtdesc.index = i;
		if (-1 == ioctl(device->fd, VIDIOC_ENUM_FMT, &fmtdesc))
		{
			break;
		}

        device->codecFmt[i] = fmtdesc.pixelformat;
        device->codecFmtCount++;
        LogI("support codec '%s' : '%4s'", fmtdesc.description,
                (char *)&fmtdesc.pixelformat);
	}

	return 0;
}
int IsSupportFmt(struct CameraDeviceS *device, uint32_t fmt)
{
	int i = 0;
	for (i = 0; i < device->codecFmtCount; i++)
	{
		if (device->codecFmt[i] == fmt)
		{
		    return 1;
		}
	}
	
    return 0;
}


struct CameraDeviceS *CameraOpen()
{
    struct CameraDeviceS *device = NULL;

    char dev_node[512] = {0};

    sprintf(dev_node, "%s", CAMERA_DEVICE);
    
    if 	((access(dev_node, F_OK)) != 0)
    {
        LogE("device '%s' not found.", dev_node);
        goto failure;
    }

    device = malloc(sizeof(*device));
    Z_ASSERT(device != NULL);
    memset(device, 0x00, sizeof(*device));
    device->fd = -1;
    
    device->fd = open(dev_node, O_RDWR|O_NONBLOCK, 0);
    if (device->fd == -1)
    {
        LogE("open device '%s' failure, errno(%d)", dev_node, errno);
        goto failure;
    }

    LogI("open camera device '%s' success.", dev_node);
    return device;
    
failure:
    if (device->fd != -1)
    {
        close(device->fd);
        device->fd = -1;
    }
    
    if (device)
    {
        free(device);
        device = NULL;
    }
    
    return NULL;

}

int CameraQueryCapability(struct CameraDeviceS *device)
{
	int ret = -1;
    //-------------------------------------------------
	// check v4l2 device capabilities
	ret = ioctl (device->fd, VIDIOC_QUERYCAP, &device->cap); 
    if (ret != 0) 
	{ 
        LogE("Error opening device: unable to query device."); 
        goto failure;
    } 
    LogI("\n"
         "\t\t\t cap.driver: '%s'\n"
         "\t\t\t cap.card: '%s'\n"
         "\t\t\t cap.bus_info: '%s'\n"             
         "\t\t\t cap.capability: '0x%08x'\n",
         device->cap.driver, device->cap.card, device->cap.bus_info, 
         device->cap.capabilities);

    if ((device->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) 
	{ 
        LogE("Error opening device: video capture not supported."); 
        goto failure;
    } 
  
    if ((device->cap.capabilities & V4L2_CAP_STREAMING) == 0) 
	{ 
        LogE("Capture device does not support streaming i/o"); 
        goto failure;
    } 

	if (!strcmp((char *)device->cap.driver, "uvcvideo"))
	{
		device->isUsbDev = 1;
	}

	// try to support this format: NV21, YUYV
	// we do not support mjpeg camera now
    ret = GetCodecFmtList(device);
    Z_ASSERT(ret == 0);
    
	if (IsSupportFmt(device, CAPTURE_IMG_FMT))
	{
		device->format = CAPTURE_IMG_FMT;
		LogI("capture format: '%4s'", (char *)&device->format);
	}
	else
	{
	    uint32_t format = CAPTURE_IMG_FMT;
		LogE("driver should not surpport '%4s' ",  (char *)&format);
		goto failure;
	}

    return 0;
    
failure:
    return -1;
}

int CameraSetCaptureParam(struct CameraDeviceS *device)
{
	int ret = -1;
	
	struct v4l2_streamparm params;
	memset(&params, 0x00, sizeof(params));
	
	params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    params.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
	params.parm.capture.capturemode = V4L2_CAP_TIMEPERFRAME;

	params.parm.capture.timeperframe.numerator = 1;
	params.parm.capture.timeperframe.denominator = 30; /* 30fps */

	ret = ioctl(device->fd, VIDIOC_S_PARM, &params);
	
	if (ret != 0)
	{
		LogE("v4l2setCaptureParams failed, '%s'", strerror(errno));
    }
	else
	{
	    
        ioctl(device->fd, VIDIOC_G_PARM, &params);
		LogI("v4l2setCaptureParams ok, numerator(%u), denominator(%u)", 
		    params.parm.capture.timeperframe.numerator,
		    params.parm.capture.timeperframe.denominator);
    }
    
	return ret;
}

int CameraSetVideoParam(struct CameraDeviceS *device)
{
    int ret = -1;
    struct v4l2_format format;

    memset(&format, 0, sizeof(format));
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 
    format.fmt.pix.width  = IMAGE_WIDTH;
    format.fmt.pix.height = IMAGE_HEIGHT;
    format.fmt.pix.pixelformat = device->format;
    format.fmt.pix.field = V4L2_FIELD_NONE;

//    format.fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
    ret = ioctl(device->fd, VIDIOC_S_FMT, &format); 
    if (ret != 0) 
    { 
        LogE("VIDIOC_S_FMT Failed: %s", strerror(errno)); 
        return ret; 
    } 

    return 0;
}

int CameraMapBuffer(struct CameraDeviceS *device)
{
    int ret = 0;
    int i;
    struct v4l2_requestbuffers reqBuf;
    struct v4l2_buffer buf;
    
    memset(&reqBuf, 0, sizeof(reqBuf));
    reqBuf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE; 
    reqBuf.memory = V4L2_MEMORY_MMAP; 
    reqBuf.count  = MAP_BUF_NUM; 
    
    ret = ioctl(device->fd, VIDIOC_REQBUFS, &reqBuf); 
    if (ret != 0) 
    { 
        LogE("Init: VIDIOC_REQBUFS failed: %s", strerror(errno)); 
        return ret;
    } 
        
    for (i = 0; i < MAP_BUF_NUM; i++)
    {
        memset(&buf, 0, sizeof(buf)); 
        buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE; 
        buf.memory = V4L2_MEMORY_MMAP; 
        buf.index  = i; 
        
        ret = ioctl(device->fd, VIDIOC_QUERYBUF, &buf); 
        if (ret != 0) 
        { 
            LogE("Unable to query buffer (%s)", strerror(errno)); 
            return ret; 
        } 

        device->mapMem[i] = mmap(NULL, buf.length, 
                                PROT_READ | PROT_WRITE, 
                                MAP_SHARED, 
                                device->fd, 
                                buf.m.offset);
        device->mapMemSize[i] = buf.length;

        if (device->mapMem[i] == MAP_FAILED)
        { 
            LogE("Unable to map buffer (%s)", strerror(errno)); 
            return -1; 
        } 
        LogI("map buffer: index(%d), offset(%u), len(%u), ret(%p)", 
                        i, buf.m.offset, buf.length, device->mapMem[i]);
                        
        // start with all buffers in queue
        ret = ioctl(device->fd, VIDIOC_QBUF, &buf); 
        if (ret != 0) 
        {
            LogE("VIDIOC_QBUF Failed"); 
            return ret; 
        } 

    }

    LogI("buffer map finish...");
    return 0;
}

int CameraStart(struct CameraDeviceS *device)
{   
    int ret = 0;
	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 

  	ret = ioctl(device->fd, VIDIOC_STREAMON, &type); 
	if (ret != 0) 
	{ 
		LogE("StartStreaming: Unable to start capture: %s", strerror(errno)); 
		return ret; 
	} 
	return ret;

}

int CameraCaptureFrame(struct CameraDeviceS *device)
{

    struct v4l2_buffer buf;
    int ret = 0;
    
    memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    //put cache from queue
    while (1)
    {
        if (0 == ioctl(device->fd, VIDIOC_DQBUF, &buf))
        {
            /* success... */
            LogI("capture one pic., buf.index(%d)", buf.index);
            break;
        }
        LogD("Fail to ioctl DQBUF, '%s', sleep 1 ms...", strerror(errno));
        usleep(1000); /* sleep 1 ms */
    }

#ifdef IMAGE_DUMP
    static int isDump = 0;
    if (!isDump)
    {
        FILE *fp;
        if((fp = fopen(DEMO_IMAGE_PATH, "w+")) == NULL)
        {
            LogE("Fail to fopen");
            return -1;
        }
        
        fwrite(device->mapMem[buf.index], device->mapMemSize[buf.index], 1, fp);
        fclose(fp);
        sync();
        isDump = 1;
    }
#endif

    ret = ioctl(device->fd, VIDIOC_QBUF, &buf);
    
    Z_ASSERT(ret == 0);

    return 0;
}

int64_t TimeNowUs()
{
    struct timeval tv;
    gettimeofday(&tv, NULL);

    return (int64_t)tv.tv_usec + tv.tv_sec * 1000000ll;
}
#if 0
int DecodeMjpeg(char *buf, int len)
{
    static VideoDecoder *vdHdr = NULL;
    if (!vdHdr)
    {
        vdHdr = CreateVideoDecoder();
        vdHdr
    }
}
#endif
int main(int argc, char *argv[])
{
    int captureNum = 1;
    struct CameraDeviceS *device;
    int ret;
    int picCount = 0;
    int64_t startTime = 0, timeNow = 0;
    
    device = CameraOpen();

    Z_ASSERT(device != NULL);

    ret = CameraQueryCapability(device);
    Z_ASSERT(ret == 0);
    
    ret = CameraSetCaptureParam(device);
    Z_ASSERT(ret == 0);

    ret = CameraSetVideoParam(device);
    Z_ASSERT(ret == 0);

    ret = CameraMapBuffer(device);
    Z_ASSERT(ret == 0);

    ret = CameraStart(device);
    Z_ASSERT(ret == 0);

    startTime = TimeNowUs();
    while (captureNum-- != 0)
    {
        ret = CameraCaptureFrame(device);    
        Z_ASSERT(ret == 0);
        picCount ++;
        timeNow = TimeNowUs();
        if (picCount == (picCount/30) * 30)
        {
            LogI("capture speed: %.2f fps", (picCount * 1000000.0)/(timeNow - startTime));
        }
    }
    return 0;
}

